/**
 * This file contains implementation of the actual viewer class.
 * The viewer uses OpenGL and GLUT wrapping to do the graphics.
 * It is based on the GLUT wrapper written by George Stetten.
 * Being licensed freely.
 *
 * @file GraphWindow.cc
 *
 * @brief basic viewer window, it does all the graphical job
 *
 * @author Tomasz Rudny (rudny@gmail.com)
 * @version 1.1
 */

#include "GraphWindow.h"
#include "ObjModel.h"
#include "DistanceComputation.h"

#include "MatVec.h"

namespace graph {

//TODO: these variables are used solely for the purpose of calculating
//and displaying the distances
ModelOBJ loader[2];
PQP_REAL points[2][3];
PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3];
PQP_Model *pqp1, *pqp2;
DistanceComputation dc;

bool showDC = false;
//end TODO


/**
 * Constructor
 *
 * @param[in] glutMaster pointer to the GlutMaster object that does all the
 * 				GLUT wrapping
 * @param[in] setWidth the desired width of the window
 * @param[in] setHeight the desired height of the window
 * @param[in] setInitPositionX the initial X coordinate of the window
 * @param[in] setInitPositionY the initial Y coordinate of the window
 * @param[in] title string to be set as the window title
 */
GraphWindow::GraphWindow(GlutMaster * glutMaster, int setWidth, int setHeight,
		int setInitPositionX, int setInitPositionY, string title) {

	width = setWidth;
	height = setHeight;

	initPositionX = setInitPositionX;
	initPositionY = setInitPositionY;

	view_rotx = 30.0f, view_roty = 0.0f, view_rotz = 50.0f;
	sx = sy = sz = 0.3f;

	mouseRButtonDown = false;
	mouseLButtonDown = false;

	scaleModeOn = false;

	wireframeMode = false;
	//-------------------------------------------------------------------------
	// GLUT specific initialization
	//-------------------------------------------------------------------------
	glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);
	glutInitWindowSize(width, height);
	glutInitWindowPosition(initPositionX, initPositionY);
	glViewport(0, 0, width, height);
	glutMaster->CallGlutCreateWindow(title.c_str(), this);
	//-------------------------------------------------------------------------
	// OpenGL custom initialization:
	//-------------------------------------------------------------------------
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glEnable(GL_DEPTH_TEST);
	glShadeModel(GL_SMOOTH | GL_COLOR);
	//-------------------------------------------------------------------------
	// lights:
	//-------------------------------------------------------------------------
	setLights();

	glColorMaterial(GL_FRONT, GL_DIFFUSE);
	//glColorMaterial(GL_FRONT,GL_SPECULAR);
	//glMateriali(GL_FRONT,GL_SHININESS,20);

	// other OpenGL custom initialization:
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_NORMALIZE);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);

	//TODO: For testing purposes:
	loader[0].loadModel("blender/cylinder.obj");
	loader[1].loadModel("blender/bone1a.obj");

	dc.loadMesh(0, &loader[0]);
	dc.loadMesh(1, &loader[1]);

	// distance computation
	R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
	R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
	R1[0][2] = R1[1][2] = R1[2][1] = 0.0;

	R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
	R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
	R2[0][2] = R2[1][2] = R2[2][1] = 0.0;

	T1[0] = 0.0;
	T1[1] = 0.0;
	T1[2] = 0.0;
	T2[0] = 0.0;
	T2[1] = 0.0;
	T2[2] = -2.0;
	dc.setRandT(R1, R2, T1, T2);
	dc.distance();
	VcV(points[0], dc.getPoint(0));
	VcV(points[1], dc.getPoint(1));

}

void GraphWindow::setLights() {
	//-------------------------------------------------------------------------
	// lights:
	//-------------------------------------------------------------------------
	GLfloat ambient[] = { 0.3f, 0.3f, 0.3f, 1.0f };
	GLfloat diffuse[] = { 1.0f, 1.0f, 1.0f, 1.0f };
	GLfloat specular[] = { 0.3f, 0.3f, 0.3f, 1.0f };

	GLfloat position0[] = { 0.0f, 0.0f, 10.0f, 1.0f };
	GLfloat position1[] = { 0.0f, 0.0f, -10.0f, 1.0f };
	GLfloat position2[] = { 10.0f, 10.0f, 0.0f, 1.0f };
	GLfloat position3[] = { -10.0f, -10.0f, 0.0f, 1.0f };

	glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, specular);
	glLightfv(GL_LIGHT0, GL_POSITION, position0);
	glEnable(GL_LIGHT0);

	glLightfv(GL_LIGHT1, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT1, GL_SPECULAR, specular);
	glLightfv(GL_LIGHT1, GL_POSITION, position1);
	glEnable(GL_LIGHT1);

	glLightfv(GL_LIGHT2, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT2, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT2, GL_SPECULAR, specular);
	glLightfv(GL_LIGHT2, GL_POSITION, position2);
	glEnable(GL_LIGHT2);

	glLightfv(GL_LIGHT3, GL_AMBIENT, ambient);
	glLightfv(GL_LIGHT3, GL_DIFFUSE, diffuse);
	glLightfv(GL_LIGHT3, GL_SPECULAR, specular);
	glLightfv(GL_LIGHT3, GL_POSITION, position3);
	glEnable(GL_LIGHT3);
}

/**
 * Destructor
 *
 * Releases all objects created.
 */
GraphWindow::~GraphWindow() {
	clearMBS();
	glutDestroyWindow(windowID);
	//TODO: for testing purposes
	loader[0].releaseModel();
	loader[1].releaseModel();
}

/** Keyboard input
 *
 * @param key
 * @param x
 * @param y
 */
void GraphWindow::CallBackKeyboardFunc(unsigned char key, int x, int y) {
	switch (key) {
	case 'w': //toggles wireframe mode on/off
		//wireframeMode = !wireframeMode;
		for(list<MbsRenderer *>::const_iterator it = renderers.begin(); it != renderers.end(); ++it)
		{
			(*it)->setWireFrameMode((wireframeMode = !wireframeMode));
		}
		glutPostRedisplay();
		break;
	case 'a':
		tempMethod1(-1.0f, 0.f);
		showDC = true;
		glutPostRedisplay();
		break;
	case 'd':
		tempMethod1(1.0f, 0.f);
		showDC = true;
		glutPostRedisplay();
		break;
	case 'z':
		tempMethod1(0.0f, -1.0f);
		showDC = true;
		glutPostRedisplay();
		break;
	case 'x':
		tempMethod1(0.0f, 1.0f);
		showDC = true;
		glutPostRedisplay();
		break;
	}
}

/**
 * Renders (using OpenGL) the scene. All drawing is here.
 */
void GraphWindow::CallBackDisplayFunc(void) {
	//-------------------------------------------------------------------------
	// OpenGL initialization operations
	//-------------------------------------------------------------------------
	glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
	glClearDepth(10000);
	glClear(GL_COLOR_BUFFER_BIT);
	glClear(GL_DEPTH_BUFFER_BIT);
	glDepthFunc(GL_LESS);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	//glOrtho(-2, 2, -2/fAspect, 2/fAspect, -10, 10);
	gluPerspective(45, 1.0, 2.0, -2.0);
	gluLookAt(0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	//-------------------------------------------------------------------------
	// Rotate the entire view based on how the user
	// dragged the mouse around
	//-------------------------------------------------------------------------
	glPushMatrix();
	glRotatef(view_rotx, 1.0f, 0.0f, 0.0f);
	glRotatef(view_roty, 0.0f, 1.0f, 0.0f);
	glRotatef(view_rotz, 0.0f, 0.0f, 1.0f);
	//-------------------------------------------------------------------------
	// zoom the scene:
	//-------------------------------------------------------------------------
	glScalef(sx, sy, sz);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	//-------------------------------------------------------------------------
	// Render the components of the scene:
	// 1. the plane grid
	// 2. the Multi Body System(s)
	// 3. the Base Frame
	//-------------------------------------------------------------------------
	net.Draw();
	// render all MBS
	for(list<MbsRenderer *>::const_iterator it = renderers.begin(); it != renderers.end(); ++it)  {
		(*it)->renderMBS();
	}
	Frame f;
	Transform t;
	f.drawBase(t, 0);

	glColor3f(0.5f, 0.5f, 0.7f);

	if (wireframeMode)
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	glColor3f(0.2f, 0.6f, 0.9f);
	if (showDC)
		loader[0].drawModel();
	glColor3f(1.0f, 0.8f, 0.3f);
	glPushMatrix();
	glTranslatef((GLfloat)T2[0], (GLfloat)T2[1], (GLfloat)T2[2]);
	if (showDC)
		loader[1].drawModel();
	glPopMatrix();
	// distance
	glBegin(GL_LINES);
	glColor3f(1.0f, 1.0f, 1.0f);
	glVertex3f((GLfloat)points[0][0], (GLfloat)points[0][1], (GLfloat)points[0][2]);
	glVertex3f((GLfloat)points[1][0], (GLfloat)points[1][1], (GLfloat)points[1][2]);
	glEnd();
	//-------------------------------------------------------------------------
	// Restore full rendering mode (if wireframe was enabled)
	//-------------------------------------------------------------------------
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glPopMatrix(); // paired with rotating the whole scene
	glutSwapBuffers();
}

void GraphWindow::CallBackReshapeFunc(int w, int h) {

	width = w;
	height = h;

	glViewport(0, 0, width, height);
	CallBackDisplayFunc();
}

void GraphWindow::CallBackIdleFunc(void) {

	//glRotatef(0.25, 1, 1, 2);
	CallBackDisplayFunc();
}

void GraphWindow::StartSpinning(GlutMaster * glutMaster) {

	glutMaster->SetIdleToCurrentWindow();
	glutMaster->EnableIdleFunction();
}

//TODO: It requires some thought - maybe we should already here check the SHIFT and CTRL keys
// so that there is no need for scaleModeOn and alike variables. Also cursor change must be
// improved
void GraphWindow::CallBackMotionFunc(int x, int y) {
	// we are in the scaling mode:
	if (glutGetModifiers() == GLUT_ACTIVE_SHIFT) {
		float s = ((float)x - (float)prevMouseX) / (float) width;

		prevMouseX = x;
		prevMouseY = y;

		sx += s;
		sy += s;
		sz += s;

		glutSetCursor(GLUT_CURSOR_INFO);
		CallBackDisplayFunc();
	} else { // rotation mode
		if (mouseLButtonDown) {
			glutSetCursor(GLUT_CURSOR_CYCLE);
			float thetaY = 360.0f * ((float) (x - prevMouseX) / (float) width);
			float thetaX = 360.0f * ((float) (prevMouseY - y) / (float) height);

			prevMouseX = x;
			prevMouseY = y;

			view_rotx += thetaX;
			view_roty += thetaY;
			CallBackDisplayFunc();
		} else if (mouseRButtonDown) {
			float thetaZ = 360.0f * ((float) (x - prevMouseX) / (float) width);

			prevMouseX = x;
			prevMouseY = y;

			view_rotz += thetaZ;
			CallBackDisplayFunc();
		}
	}
}

void GraphWindow::CallBackMouseFunc(int button, int state, int x, int y) {
	if (glutGetModifiers() == GLUT_ACTIVE_SHIFT) {
		prevMouseX = x;
		prevMouseY = y;
		scaleModeOn = true;
		glutSetCursor(GLUT_CURSOR_INFO);
	} else {
		scaleModeOn = false;
		if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
			prevMouseX = x;
			prevMouseY = y;
			mouseLButtonDown = true;
			glutSetCursor(GLUT_CURSOR_CYCLE);
		} else if (button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN) {
			prevMouseX = x;
			prevMouseY = y;
			mouseRButtonDown = true;
			glutSetCursor(GLUT_CURSOR_CYCLE);
		} else {
			mouseRButtonDown = false;
			mouseLButtonDown = false;
			glutSetCursor(GLUT_CURSOR_INHERIT);
		}
	}
}

void GraphWindow::setMBS(RSIM::MultibodySystem * mbs) {
	renderer->setMBS(mbs);
}

/**
 * Adds a pointer to a MBS object. Each added MBS will then be
 * rendered using a proper renderer class.
 *
 * @param mbs a pointer to the RSIM::MultibodySystem object
 */
void GraphWindow::addMBS(RSIM::MultibodySystem * mbs) {
	ObjectMbsRenderer * oren = new ObjectMbsRenderer;
	oren->setMeshCount(1);
	oren->loadMesh("blender/default.obj", 0);

	for (int j = 0; j < mbs->getNNodes(); j++) {
		oren->assignMesh(j, 0);
	}

	oren->setMBS(mbs);

	renderers.push_front(oren);
}

/**
 * A more complicated version of adding an MBS and its
 * renderer.
 *
 * Here it can be specified, what .OBJ files should be loaded
 * and used for each body. This is done in the following way:
 *
 * model_files[] array contains file names of all models to be
 * loaded. The modelFileCount is its size. Then the next array
 * (of the size getNNodes()) contains indexes of models to be
 * assigned for each node.
 *
 * @param[in] mbs a MBS object
 * @param[in] model_files[] an array of file names of models
 * 	to be loaded
 * @param[in] modelFileCount the size of model_files[]
 * @param[in] modelAssignments an array containing information
 * 	which model should be used for each node
 *
 * Example:
 * 	modelAssignments = { 1, 0, 1, 2, 1 }
 * means that model #1 will be used for nodes 0, 2 and 4,
 * model #0 will be used for node 1 and model #2 for node 3.
 *
 */
void GraphWindow::addMBS(RSIM::MultibodySystem * mbs,
		std::string model_files[], int modelFileCount,
		int modelAssignments[]) {

	ObjectMbsRenderer * oren = new ObjectMbsRenderer;
	oren->setMeshCount(modelFileCount);

	int j;

	for (j = 0; j < modelFileCount; j++)
		oren->loadMesh(model_files[j].c_str(), j);

	for (j = 0; j < mbs->getNNodes(); j++) {
		oren->assignMesh(j, modelAssignments[j]);
	}

	oren->setMBS(mbs);

	renderers.push_front(oren);
}

/**
 * Clears all renderers and MBS objects.
 */
void GraphWindow::clearMBS() {
	for(list<MbsRenderer *>::const_iterator it = renderers.begin(); it != renderers.end(); ++it)
	{
		delete *it;
	}
	renderers.clear();
}

void GraphWindow::tempMethod1(double shiftX, double shiftZ) {

	R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
	R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
	R1[0][2] = R1[1][2] = R1[2][1] = 0.0;

	R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
	R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
	R2[0][2] = R2[1][2] = R2[2][1] = 0.0;

	T1[0] = 0.0;
	T1[1] = 0.0;
	T1[2] = 0.0;
	T2[0] += shiftX;
	T2[1] = 0.0;
	T2[2] += shiftZ;

	dc.setRandT(R1, R2, T1, T2);
	dc.distance();
	VcV(points[0], dc.getPoint(0));
	VcV(points[1], dc.getPoint(1));

}

}
